#!/usr/bin/env python
# -*- coding: utf-8 -*-
'''pose_manager ROS Node'''
# license removed for brevity
import rospy
from std_msgs.msg import String
from manager.srv import *
from manager.msg import *
from slamwareAPP.srv import *

from pymongo import MongoClient
import json
import xlrd

pub_guide = rospy.Publisher('guide', Pose, queue_size=10)
pub_unknow = rospy.Publisher('tts', String, queue_size=10)
pub_handup = rospy.Publisher('handup', String, queue_size=10)
pub_handfree = rospy.Publisher('handfree', String, queue_size=10)

rospy.wait_for_service('/slamwareNode/get_pose')
pose_service = rospy.ServiceProxy('/slamwareNode/get_pose', GetPoseSrv)

def hand_free(event):
    pub_handfree.publish("")

def handle_add_pose(req):

    robot_pose = pose_service()

    db = MongoClient().eveapitest
    db.pose.insert({
        "name": req.pose.name,
        "x":robot_pose.x,
        "y": robot_pose.y,
        "yaw": robot_pose.yaw,
    })
    return AddPoseResponse(True)

def handle_del_pose(req):
    # print "del"
    pose = Pose()
    db = MongoClient().eveapitest
    ret = db.pose.remove({"name": req.name})
    # encodedjson = json.dumps(p)    
    return DelPoseResponse(True)


def handle_get_pose(req):
    # print "handle_get_pose"
    pose = Pose()
    db = MongoClient().eveapitest
    p = db.pose.find_one({"name": req.name}, {"name": 1, "x": 1, "y": 1, "yaw": 1})
    # encodedjson = json.dumps(p)
    if p:
        pose.name = p["name"]
        pose.x = p["x"]
        pose.y = p["y"]
        pose.yaw = p["yaw"]
    return GetPoseResponse(pose)

def handle_get_all_poses(req):
    # print "handle_get_pose"
    arr = []
    db = MongoClient().eveapitest
    p = db.pose.find({}, {"name": 1, "x": 1, "y": 1, "yaw": 1})
    for item in p:    #打印数据
        pose = Pose()
        pose.name = item["name"]
        pose.x = item["x"]
        pose.y = item["y"]
        pose.yaw = item["yaw"]
        arr.append(pose)
    return GetAllPosesResponse(arr)

# def handle_move_to(req):

#     pose = Pose()
#     db = MongoClient().eveapitest
#     p = db.pose.find_one({"name": "{0}".format(req.name)}, {"name": 1, "x": 1, "y": 1, "yaw": 1})
#     if p:
#         pose.name = req.name
#         pose.x = p["x"]
#         pose.y = p["y"]
#         pose.yaw = p["yaw"]
#         pub_guide.publish(pose)
#         # pub_handup.publish("")
#         # rospy.Timer(rospy.Duration(3), hand_free, oneshot=True)
#         # pub_unknow.publish("好的,跟我走吧")
#         return MoveToResponse(True)
#     else:
#         # pub_unknow.publish(path_tip.encode('utf-8'))
#         pub_unknow.publish("没有这个地方")
#         return MoveToResponse(False)

#     # print "move_to"
#     return MoveToResponse(True)

def handle_move_to(req):

    room_maps = {}
    data = xlrd.open_workbook('table.xlsx') # 打开xls文件
    table = data.sheets()[0] # 打开第一张表
    nrows = table.nrows # 获取表的行数
    for i in range(nrows): # 循环逐行打印
        row_values = table.row_values(i)[:4]
        room     = row_values[0]
        text     = row_values[1]
        position = int(row_values[2])
        _id      = row_values[3]
        _str = "{0}".format(int(_id))
        room_maps[_str] = {'text': text, 'position': position}

    # print room_maps[req.name]
    name = room_maps[req.name]["position"]
    path_tip = room_maps[req.name]["text"]
    # area_id = 

    pose = Pose()
    db = MongoClient().eveapitest
    p = db.pose.find_one({"name": "{0}".format(name)}, {"name": 1, "x": 1, "y": 1, "yaw": 1})
    if p:
        pose.name = req.name
        pose.x = p["x"]
        pose.y = p["y"]
        pose.yaw = p["yaw"]
        pub_guide.publish(pose)
        # pub_handup.publish("")
        # rospy.Timer(rospy.Duration(3), hand_free, oneshot=True)
        # pub_unknow.publish("好的,跟我走吧")
        return MoveToResponse(True)
    else:
        # pub_unknow.publish(path_tip.encode('utf-8'))
        pub_unknow.publish("没有这个地方")
        return MoveToResponse(False)

    # print "move_to"
    return MoveToResponse(True)

def excu_tts(data):
    pub_unknow.publish(data.data)

def arrive_func(data):
    room_maps = {}
    table_data = xlrd.open_workbook('table.xlsx') # 打开xls文件
    table = table_data.sheets()[0] # 打开第一张表
    nrows = table.nrows # 获取表的行数
    for i in range(nrows): # 循环逐行打印
        row_values = table.row_values(i)[:4]
        room     = row_values[0]
        text     = row_values[1]
        position = row_values[2]
        _id      = row_values[3]
        _str = "{0}".format(int(_id))
        room_maps[_str] = {'text': text, 'position': position}

    # name = room_maps[req.name]["position"]
    path_tip = room_maps[data.data]["text"]
    pub_unknow.publish(path_tip.encode('utf-8'))
    pub_handup.publish("")
    rospy.Timer(rospy.Duration(2), hand_free, oneshot=True)

def talker():
    '''pose_manager Publisher'''
    # pub = rospy.Publisher('pub_all_pose', String, queue_size=10)
    rospy.init_node('pose_manager_pub', anonymous=True)
    rospy.Service('add_pose', AddPose, handle_add_pose)
    rospy.Service('del_pose', DelPose, handle_del_pose)
    rospy.Service('get_pose', GetPose, handle_get_pose)
    rospy.Service('get_all_poses', GetAllPoses, handle_get_all_poses)
    rospy.Service('move_to', MoveTo, handle_move_to)
    rospy.Subscriber('write', String, excu_tts)
    rospy.Subscriber('arrive', String, arrive_func)
    rate = rospy.Rate(1)  # 10hz

    while not rospy.is_shutdown():
    
        rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass
